mobrob - A1

In der Vorlesung sowie in den Übungen wurden zwei Dreirad-Konfigurationen erörtert. Eine der zwei Dreirad-Konfigurationen hatte ein angetriebenes, lenkbares Vorderrad (Dreirad 1). Im Skript wurde hierzu die Pfadintegration wie folgt hergeleitet (siehe Skript Seite 7):

\begin{align} \newcommand{\s}{\operatorname{s}} \newcommand{\c}{\operatorname{c}} \dot{\mathbf{X}}= \left( \begin{matrix} \dot{x}\\ \dot{y}\\ \dot{\theta} \end{matrix} \right) = \left( \begin{matrix} v \s_{\beta} \c_{\theta} \\ v \s_\beta \s_\theta\\ -\frac{1}{L} v \c_{\beta} \end{matrix} \right) \end{align}

Die Übungsaufgabe 13 aus dem Aufgabenheft dient als Grundlage für die Implementierung des Dreirad 1 mit hilfe des Roboter-Simulators. Eine numerische Näherung für die Odometrie ist hierbei unter Anwendung des Polygonzug-Verfahrens (Euler-Colaz) wie folgt herleitbar:

\begin{align} \newcommand{\s}{\operatorname{s}} \newcommand{\c}{\operatorname{c}} \mathbf{X}_{k+1}= \operatorname{\mathbf{F}}(x,y,\theta , \beta , s)= \begin{pmatrix} x\\ y\\ \theta \end{pmatrix} + \begin{pmatrix} s \s_\beta \cos\left( \theta - \frac{s \c_\beta}{2L}\right)\\ s \s_\beta \sin\left( \theta - \frac{s \c_\beta}{2L}\right)\\ -\frac{s \c_\beta}{L} \end{pmatrix} \text{ mit } s=v\Delta t \end{align}

Lösung

Nachfolgend wird die Berechnung der Fehlerellipse angegeben. Fehlerfortpflanzung der Odometrie

\begin{align} \mathbf{C}_{\text{P},k+1,k} = \mathbf{F}_{\text{P},k+1} \mathbf{C}_{\text{P},k,k} \mathbf{F}_{\text{P},k+1}^{\text{T}} + \mathbf{F}_{\text{S},k+1} \mathbf{C}_{\text{S},k+1} \mathbf{F}_{\text{S},k+1}^{\text{T}} \end{align}

Notiz:

$\mathbf{F}_{\text{P},k+1}$ ist die Jacobi-Matrix nach der Position $\mathbf{X}$ im Arbeitspunkt $k+1$ mit $\mathbf{F}_{\text{P},k+1} = \frac{\partial\mathbf{X}_{k+1}}{\partial\mathbf{X}}$

$\mathbf{F}_{\text{S},k+1}$ ist die Jacobi-Matrix nach der Geschwindigkeit $\mathbf{v}$ im Arbeitspunkt $k+1$ mit $\mathbf{F}_{\text{S},k+1} = \frac{\partial\mathbf{X}_{k+1}}{\partial\mathbf{v}}$

$\mathbf{C}_{\text{S},k+1}$ ist die Unsicherheit der zurückgelegten Wegstrecke, welche mit der Geschwindigkeit $\mathbf{v}$ skaliert (Die Konstanten $(k_1,k_2)$ müssen messtechnisch ermittelt werden) mit $\mathbf{C}_{\text{S},k+1}=\text{diag}\left(k_1 \left| s_{k+1}\right|, k_2 \left| \Delta\beta_{k+1}\right| \right)$

Lösung der Matrizen:

\begin{align} \mathbf{F}_{\text{P},k+1}= \mathbf{H}_{\text{P},k+1} = \left. \frac{\partial \mathbf{F}}{ \partial \mathbf{X} }\right|_{\mathbf{X}_{k+1}} = \left( \begin{matrix} 1 & 0 & - s \s_\beta \sin\left( \theta - \frac{s \c_\beta}{2L}\right)\\ 0 & 1 & s \s_\beta \cos\left( \theta - \frac{s \c_\beta}{2L}\right)\\ 0 & 0 & 1 \end{matrix} \right) \end{align}

\begin{align} \mathbf{F}_{\text{S},k+1}= \mathbf{H}_{\text{S},k+1} = \left. \frac{\partial \mathbf{F}}{ \partial (v,\beta)^{\text{T}} }\right|_{\mathbf{X}_{k+1}} = \begin{pmatrix} \mathbf{F}_{\text{S}_{v},k+1} & \mathbf{F}_{\text{S}_{\beta},k+1} \end{pmatrix} \end{align}

mit

\begin{align} \mathbf{F}_{\text{S}_{v},k+1} = s_\beta \Delta t \begin{pmatrix} \cos\left( \theta - \frac{s \c_\beta}{2L}\right) + \frac{s}{2L}\c_\beta \sin\left( \theta - \frac{s \c_\beta}{2L}\right) \\ \sin\left( \theta - \frac{s \c_\beta}{2L}\right) - \frac{s}{2L}\c_\beta \cos\left( \theta - \frac{s \c_\beta}{2L}\right) \\ - \frac{\arctan(\beta)}{L} \end{pmatrix} \end{align}

\begin{align} \mathbf{F}_{\text{S}_{\beta},k+1} = s \begin{pmatrix} \c_\beta \cos\left( \theta - \frac{s \c_\beta}{2L}\right) - \frac{s}{2L} \s_\beta \sin\left( \theta - \frac{s \c_\beta}{2L}\right) \\ \c_\beta \sin\left( \theta - \frac{s \c_\beta}{2L}\right) + \frac{s}{2L} \s_\beta \cos\left( \theta - \frac{s \c_\beta}{2L}\right)\\ \frac{\s_\beta}{L} \end{pmatrix} \end{align}